#include "os_task.h"
#include "os_sched.h"
#include "os_list.h"
#include "os_event.h"
#include "os_cfg.h"
#include "os_plat.h"
#include "os_mem.h"
#include "os_timer.h"
#include "os_idle.h"

os_core_t  os_core;          // OS核心结构

#if OS_RDY_BITMAP_CHECK_ENABLE || OS_SCHED_DEBUG_ENABLE || OS_SCHED_CHECK_TASK_STACK
#undef OS_DBG_PRINT_ENABLE
#define OS_DBG_PRINT_ENABLE     1       // 强制允许输出
#undef OS_ASSERT_ENABLE
#define OS_ASSERT_ENABLE        1       // 允许assert
#endif

/**
 * 检查位图的正确性
*/
#if OS_RDY_BITMAP_CHECK_ENABLE
void os_bitmap_check (void) {

    // 不计算idle所在的最低优先级队列
    for (int i = 0; i <= OS_TASK_PRIO_MAX; i++) {
        // 检查位的设置
        int group = i >> 3;
        int group_map = os_core.ready_map[group];
        if (os_core.ready_group & (1 << group)) {
            os_assert(group_map != 0);
        } else {
            // 没有任务在里面
            os_assert(group_map == 0);
        }

        os_list_t * list = &os_core.ready_list[i];
        if (group_map & (1 << (i & 0x7))) {
            os_assert(os_list_first(list) != OS_NULL);
        } else {
            os_assert(os_list_first(list) == OS_NULL);
        }
    }        

    // 最低优先级必须总是置1
    os_assert(os_core.ready_map[(OS_TASK_PRIO_MAX) >> 3] != 0);
}
#else
#define os_bitmap_check(bitmap)
#endif

/**
 * 获取下一高优先级的可运行的任务
*/
static os_task_t * sched_next_run_task (void) {
	static const uint8_t map_table[] = {
	    /* 00 */ 0xff, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
	    /* 10 */ 4,    0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
	    /* 20 */ 5,    0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
	    /* 30 */ 4,    0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
	    /* 40 */ 6,    0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
	    /* 50 */ 4,    0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
	    /* 60 */ 5,    0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
	    /* 70 */ 4,    0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
	    /* 80 */ 7,    0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
	    /* 90 */ 4,    0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
	    /* A0 */ 5,    0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
	    /* B0 */ 4,    0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
	    /* C0 */ 6,    0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
	    /* D0 */ 4,    0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
	    /* E0 */ 5,    0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
	    /* F0 */ 4,    0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0
	};

    uint8_t group = map_table[os_core.ready_group];                      // 取就绪组号
    os_assert((group >= 0) && (group <= 8));
    uint8_t prio = (group << 3) + map_table[os_core.ready_map[group]];   //取就绪位

    // 如果是自己返回NULL
    os_task_t * to = (os_task_t *)os_list_first(os_core.ready_list + prio);
    return (to != os_core.curr_task) ? to : OS_NULL;
}

/**
 * 返回当前任务
 */
os_task_t * os_task_self (void) {
    return os_core.curr_task;
}

/**
 * 返回系统启动后的时钟周期数
 */
os_ticks_t os_tick_count(void) {
    os_int_status_t status = os_sched_int_lock();
    os_ticks_t ticks = os_core.startup_ticks;
    os_sched_int_restore(status);

    return ticks;
}

#if OS_SCHED_LOCK_ENABLE
/**
 * 禁止任务调度
 */
void os_sched_lock (void) {
    os_int_status_t status = os_sched_int_lock();

    if (os_core.sched_locked < 255) {
        os_core.sched_locked++;
    }

    os_sched_int_restore(status);
}

/**
 * 允许任务调度
 */
void os_sched_unlock (void) {
    os_int_status_t status = os_sched_int_lock();

    if (os_core.sched_locked > 0) {
        if (--os_core.sched_locked == 0) {
            // 可能有更高优先级的任务就绪，因此进行一次调度
            // 也有可能任务调用了其它的阻塞函数，暂时执行，那么就可以在这里切换
            os_sched_run(); 
        }
    }

    os_sched_int_restore(status);
}

/**
 * 返回调度锁锁定的次数
*/
int os_sched_locked_count (void) {
    os_int_status_t status = os_sched_int_lock();
    int cnt = os_core.sched_locked;
    os_sched_int_restore(status);

    return cnt;
}
#endif

/**
 * 出栈前的任务栈空间检查
 */
#if OS_SCHED_CHECK_TASK_STACK
int os_sched_stack_check (os_task_t * task) {
    // 第一步：检查ctx指针指向的范围, ctx不能超过两端
    uint8_t * stack_end = (uint8_t *)task->start_stack + task->stack_size;
    if ((uint8_t *)task->ctx >= stack_end) {
        os_dbg("task %s stack check failed: ctx = %d > stack_top = %d\n",
                        task->name, (int)task->ctx,(int)stack_end);
        return -1;
    }

    uint8_t * stack_start = (uint8_t *)task->start_stack;
    if ((uint8_t *)task->ctx <= stack_start) {
        os_dbg("task %s stack check failed: ctx = %d < stack_top = %d\n",
                        task->name, (int)task->ctx, (int)stack_end);
        return -1;
    }

    // 准备运行的任务，肯定能弹栈，但是用户的剩余空间不一定够
    int free_bytes = 0;
#if OS_PLAT_STACK_GROW_HTL
    uint8_t * start = (uint8_t *)task->start_stack;
    for (int i = 0; i < task->stack_size; i++) {
        if (*start++ != OS_TASK_STACK_FILL) {
            break;
        }
        free_bytes++;;
    }
#else
    uint8_t * start = task->start_stack + task->stack_size - 1;
    for (int i = 0; i < info->stack_size; i++) {
        if (*start-- != OS_TASK_STACK_FILL) {
            break;
        }
        free_bytes++;;
    }
#endif
    task->stack_used_max = task->stack_size - free_bytes;
    if (free_bytes < OS_SCHED_STACK_THRESHOLD ) {
        os_dbg("task %s stack check failed: unused = %d < threshold = %d\n",
                        task->name, free_bytes, OS_SCHED_STACK_THRESHOLD);
        return -1;
    }
    return 0;
}
#else
#define os_sched_stack_check(task)              0
#endif

/**
 * 运行一次任务调试器，做一次任务切换
 */
void os_sched_run (void) {   
    os_int_status_t status = os_sched_int_lock();

    // 已经上锁，禁止调度
#if OS_SCHED_LOCK_ENABLE
    if (os_core.sched_locked > 0) {
        os_sched_int_restore(status);
        return;
    }
#endif

    // 中断嵌套中，不允许任务切换
    if (os_core.int_nested > 0) {
        return;
    }

    // 取最高优先级任务
    // 也许别的任务误改了，所以这里即便没改，也检查下
    os_bitmap_check();

    os_task_t * to;             // 不能放下边，编译会出错
#if OS_SCHED_CHECK_TASK_STACK
rescheduler:
#endif
    to = sched_next_run_task();
    if (to == OS_NULL) {
        os_core.sched_from_int = 0;
        os_sched_int_restore(status);
        return;
    }
    
    // 显示任务切换信息
#if OS_SCHED_DEBUG_ENABLE
    os_dbg("switch from %s(%d) to %s(%d)\n", os_core.curr_task->name,
                        os_core.curr_task->ctx, to->name, to->ctx);
#endif
    // 待运行的任务进行出栈前的检查
#if OS_SCHED_CHECK_TASK_STACK
    // 下一任务切换检查，如果出错，则需要跳过该任务
    os_err_t err = os_sched_stack_check(to);
    if (err < 0) {
        if (to->prio == OS_TASK_PRIO_MAX) {
            // 空闲任务不能挂起
            os_dbg("to %s: stack check failed, continue to run \n", to->name);
        } else {
            os_dbg("to %s: stack check failed, suspend this next and try next \n", to->name);
            os_sched_remove_ready(to);
            //os_sched_set_suspend(to);
            goto rescheduler;
        }
    }
#endif

// 任务切换HOOK
#if OS_TASK_SWITCH_HOOK_ENABLE
    os_task_switch_hook(os_core.curr_task, to);
#endif

    // 不能在这里加个。因为可能还没切换后就又发生了中断，然后又进入了sched
    // 然后又执行上面的代码，而此时from已经改变了，但实际未改变，进而导致压栈失败
    os_core.next_task = to;  
    if (os_core.sched_from_int) {
        os_int_switch(os_core.curr_task->ctx, os_core.next_task->ctx);
    } else {
        os_task_switch(os_core.curr_task->ctx, os_core.next_task->ctx);
    }

    os_sched_int_restore(status);
}

/**
 * 初始化就绪位置
 * @param bitmap 位图
 */
static void sched_init_ready_list (void) {    
    // 位图初始化
    os_core.ready_group = 0;
    for (int i = 0; i < (int)sizeof(os_core.ready_map); i++) {
        os_core.ready_map[i] = 0;
    }

    // 就绪队列
    os_core.curr_task = OS_NULL;
    os_core.next_task = OS_NULL;
    for (int i = 0; i <= OS_TASK_PRIO_MAX; i++) {
        os_list_init(&os_core.ready_list[i], os_task_t, run_item);
    }
}

/**
 * 将任务设置为就绪状态
 */
void os_sched_set_ready (os_task_t * task) {
    os_assert(task != OS_NULL);
    os_assert((task->prio >= 0) && (task->prio <= OS_TASK_PRIO_MAX));

    os_list_insert_last(os_core.ready_list + task->prio, task);

    int group = task->prio >> 3;
    os_core.ready_group |= 1 << group;
    os_core.ready_map[group] |= 1 << (task->prio & 0x7);

    task->flags |= OS_TASK_STATE_RDY;

    os_bitmap_check();
}

/**
 * 将任务从就绪列表中移除
 */
void os_sched_remove_ready (os_task_t * task) {
    os_assert(task != OS_NULL);
    os_assert((task->prio >= 0) && (task->prio <= OS_TASK_PRIO_MAX));

    os_list_t * list = os_core.ready_list + task->prio;

    os_list_remove_item(list, task);
    task->flags &= ~OS_TASK_STATE_RDY;

    if (os_list_first(list) == OS_NULL) {
       int group = task->prio >> 3;

        os_core.ready_map[group] &= ~(1 << (task->prio & 0x7));
        if (os_core.ready_map[group] == 0) {
            os_core.ready_group &= ~(1 << group);
        }

        os_bitmap_check();
    }
}

#if OS_SCHED_DEBUG_ENABLE
static void delay_list_show (void) {
    // 遍历打开整个链表，用于调试
    os_list_for_each(curr_task, os_task_t, &os_core.delay_list) {
        os_dbg("%s: ticks = %d\n", curr_task->name, curr_task->delay_tick);
    }
}
#else
#define delay_list_show()
#endif

/**
 * 设置进入延时状态
 */
void os_sched_set_delay (os_task_t * task, int ms) {
    int ticks = (ms + OS_SYSTICK_MS - 1) / OS_SYSTICK_MS;
    if (ticks <= 0) {
        ticks = 1;
    }
    task->delay_tick = ticks;
    task->flags |= OS_TASK_STATE_DELAYED;

#if OS_SCHED_DELAY_INSERT_LAST
    // 直接插入到队尾
    os_list_insert_last(&os_core.delay_list, task);
#else
    // 遍历链表，插入至合适的位置
    os_list_for_each(curr, os_task_t, &os_core.delay_list) {
        if (curr->delay_tick < task->delay_tick) {
            // 比当前任务延时要大
            task->delay_tick -= curr->delay_tick;
            continue;
        } else if (curr->delay_tick == task->delay_tick) {
            task->delay_tick = 0;
            os_list_insert_after(&os_core.delay_list, curr, task);
            delay_list_show();
           return;
        } else {
            // 比当前任务延时要小
            curr->delay_tick -= task->delay_tick;

            os_task_t * pre_task = os_list_item_pre(&os_core.delay_list, curr);
            os_list_insert_after(&os_core.delay_list, pre_task, task);
            delay_list_show();
            return;
        }
    }
    // curr为空，显然是队列中没有
    os_list_insert_last(&os_core.delay_list, task); 
#endif
    delay_list_show();
}

/**
 * 将延时的任务从延时队列中唤醒
 */
void os_sched_remove_delay (os_task_t * task) {
#if !OS_SCHED_DELAY_INSERT_LAST
    // 移植时，需要考虑其对前后结点的影响
    os_task_t * next_task = (os_task_t *)os_list_item_next(&os_core.delay_list, task);
    if (next_task) {
        next_task->delay_tick += task->delay_tick;
    }
#endif

    os_list_remove_item(&os_core.delay_list, task);
    task->flags &= ~OS_TASK_STATE_DELAYED;
}

/**
 * 将任务加入挂起列表
*/
#if OS_TASK_SUSPEND_ENABLE
void os_sched_set_suspend (os_task_t * task) {
    os_list_insert_last(&os_core.pend_list, task);
    task->flags |= OS_TASK_STATE_SUSPEND;
}

/**
 * 将任务从挂起列表中移除
*/
void os_sched_remove_suspend (os_task_t * task) {
    os_list_remove_item(&os_core.pend_list, task);
    task->flags &= ~OS_TASK_STATE_SUSPEND;
}
#endif

/**
 * 系统时钟节拍处理
 */
void os_sched_time_tick (void) {    
    os_int_status_t status = os_sched_int_lock();
    
#if OS_SCHED_DELAY_INSERT_LAST
    os_task_t * task = (os_task_t *)os_list_first(&os_core.delay_list);
    while (task) {
        os_task_t * next = (os_task_t *)os_list_item_next(&os_core.delay_list, task);
        if (--task->delay_tick == 0) {
            if (task->flags & OS_TASK_STATE_WAIT) {
                os_event_wakeup_task(task->event_wait.event, task, OS_NULL, OS_ERR_TMO);
            } else {
                os_sched_remove_delay(task);
                os_sched_set_ready(task);
            }
        }

        // 取下一任务
        task = next;
    }
#else
    // 处理定时到的情况，保需要检查第一个非0的表项和后面全为0的表项
    os_task_t * task = (os_task_t *)os_list_first(&os_core.delay_list);
    if (task && (--task->delay_tick == 0)) {
        do {
            os_task_t * next = (os_task_t *)os_list_item_next(&os_core.delay_list, task);

            // 唤醒任务，通知超时
            if (task->flags & OS_TASK_STATE_WAIT) {
                os_event_wakeup_task(task->event_wait.event, task, OS_NULL, OS_ERR_TMO);
            } else {
                os_sched_remove_delay(task);
                os_sched_set_ready(task);
            }

            // 取下一个任务
            task = next;
        } while (task && (task->delay_tick == 0));
    }
#endif // OS_SCHED_DELAY_INSERT_LAST


    // 检查当前任务的时间片，注意当前任务可能在发生中断之前转到其它状态
#if OS_TASK_SCHED_RR
    if (os_core.curr_task && (os_core.curr_task->flags & OS_TASK_STATE_RDY)) {
        if (--os_core.curr_task->slice <= 0) {
            os_core.curr_task->slice = OS_TASK_SLICE;        // 重置计数

            // 如果有后续任务，切换到后面的任务，后一个任务再上前
            if (os_core.curr_task->run_item.next) {          
                os_list_t * task_list = os_core.ready_list + os_core.curr_task->prio;
                os_list_remove_item(task_list, os_core.curr_task);
                os_list_insert_last(task_list, os_core.curr_task);
            }
        }
    }
#endif

    // 节拍计数增加
    os_core.startup_ticks++;

    os_sched_int_restore(status);

#if OS_SYSTICK_HOOK_ENABLE
    os_systick_hook();
#endif
}

/**
 * 将任务加入到删除队列中
 */
#if OS_TASK_DEL_ENABLE
void os_sched_set_deleted(os_task_t * task) {
    os_list_insert_last(&os_core.delete_list, task);
}

/**
 * 从待删除队列中移除一个任务
*/
os_task_t * os_sched_remove_deleted (void) {
    os_int_status_t status = os_sched_int_lock();
    os_task_t * task = (os_task_t *)os_list_remove_first(&os_core.delete_list);
    os_sched_int_restore(status);
    return task;
}
#endif

/**
 * 加入新创建的任务
 */
void os_sched_add_new (os_task_t * task) {
    os_list_insert_last(&os_core.all_list, task);
}

/**
 * 移除新创建的任务
 */
void os_sched_remove (os_task_t * task) {
    os_list_remove_item(&os_core.all_list, task);
}

/**
 * 保存当前栈的指针并加载新栈的指针
 */
os_task_ctx_t *os_sched_switch_ctx(os_task_ctx_t *ctx) {
    if (ctx != (os_task_ctx_t *)0) {
        os_core.curr_task->ctx = ctx;

    // 压栈后，应该还要有一些预留的空间，保证程序运行正常
#if OS_SCHED_CHECK_TASK_STACK
        uint8_t * stack_start = (uint8_t *)os_core.curr_task->start_stack;
        uint8_t * stack_end = stack_start + os_core.curr_task->stack_size;
        uint8_t * curr_ctx = (uint8_t *)os_core.curr_task->ctx;
        // 空间必须有效，不然栈肯定出错了。。。。
        os_assert(curr_ctx > stack_start);
        os_assert(curr_ctx < stack_end);

    #if OS_PLAT_STACK_GROW_HTL
        int free_bytes = (int)(curr_ctx - stack_start);
    #else
        int free_bytes = (int)(stack_end - curr_ctx);

    #endif
        if (free_bytes < OS_SCHED_STACK_THRESHOLD ) {
            os_dbg("warning: task %s stack check failed: free = %d < threshold = %d\n",
                            os_core.curr_task->name, free_bytes, OS_SCHED_STACK_THRESHOLD);
        }
#endif
    }

    // 出栈前的检查在os_sched中完成
    os_core.curr_task = os_core.next_task;
    return os_core.next_task->ctx;
}

/**
 * 初始化调试相关
*/
void os_sched_init (void) {
    sched_init_ready_list();
    os_list_init(&os_core.all_list, os_task_t, info_item);

#if OS_TASK_DEL_ENABLE
    os_list_init(&os_core.delete_list, os_task_t, run_item);
#endif
#if OS_TASK_SUSPEND_ENABLE
    os_list_init(&os_core.pend_list, os_task_t, run_item);
#endif
    os_list_init(&os_core.delay_list, os_task_t, run_item);

#if OS_SCHED_LOCK_ENABLE
    os_core.sched_locked = 0;       // 调度锁
#endif

    os_core.startup_ticks = 0;
    os_core.int_nested = 0;
    os_core.sched_from_int = 0;
}

/**
 * 进入关中断保护状态
 */
os_int_status_t os_sched_int_lock(void) {
    return os_plat_int_disable();
}

/**
 * 退出关中断保护状态
 */
void os_sched_int_restore(os_int_status_t status) {
    os_plat_int_restore(status);
}

/**
 * 进入中断处理函数
 */
void os_int_enter (void) {
    os_int_status_t status = os_sched_int_lock();
    os_core.int_nested++;
    os_sched_int_restore(status);
}

/**
 * 退出中断处理函数
 */
void os_int_leave (void) {
    os_int_status_t status = os_sched_int_lock();
    os_core.int_nested--;
    if (os_core.int_nested == 0) {
        // 从中断中发生的任务切换
        os_core.sched_from_int = 1;
        os_sched_run();
    }
    os_sched_int_restore(status);
}

/**
 * 执行第一个任务
 */
void os_sched_run_first (void) {
    // 取下一个将要运行的任务
    os_core.next_task = sched_next_run_task();
    os_core.curr_task = os_core.next_task;
    os_assert(os_core.curr_task != OS_NULL);

    // 切换到第一个任务
    os_dbg("switching to first task\n");
    os_task_switch_to(os_core.next_task->ctx);
}

/**
 * 释放CPU给同一优先级的其它任务
 */
#if OS_TASK_SCHED_RR
void os_sched_yield (void) {
    os_task_t * self = os_task_self();

    os_int_status_t status = os_sched_int_lock();

    os_list_t * list = &os_core.ready_list[self->prio];
    os_task_t * next = (os_task_t *)os_list_item_next(list, self);
    if (next && (next != self)) {        
        // 注意调整下时间片, 因为放弃了运行，调整到后边
        self->slice = OS_TASK_SLICE;

        // 有后续结点，将自己移到表尾，这样后一个就前面来了
        os_sched_remove_ready(self);
        os_sched_set_ready(self);

        // 然后切换过去
        os_sched_run();
    }
    
    os_sched_int_restore(status); 
}
#endif

